/**
 * @brief 常用变换
 * @version 1.0
 * @date 2023-09-22
 *
 */

#ifndef _ALGORITHM_H_
#define _ALGORITHM_H_

#include "stdio.h"
#include <iostream>
#include <deque>
#include <vector>
#include <unistd.h>
#include <sys/socket.h>
#include <cmath>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <array>
#include <functional>
#include <chrono>
#include <queue>



using namespace std;
using namespace Eigen;
using namespace chrono;

/**
 * @brief 角度转化为弧度
 * @param deg 角度
 */
double deg2rad(double deg);

/**
 * @brief 弧度转化为角度
 * @param rad 弧度
 */
double rad2deg(double rad);

/**
 * @brief 关节角vector转换为rad
 * @param joint_in_degree
 */
void joint2rad(std::vector<double> &joint_in_degree);


/**
 * @brief 旋转矩阵转化为欧拉角 pass
 * @param rot 旋转矩阵
 * @param zyx 输出结果：按照z-y-x顺序
 */
void rot2euler(Matrix3d rotation_matrix, Vector3d &zyx);

/**
 * @brief 旋转矩阵转换为rpy角
 * @param rotation_matrix
 * @return
 */
Vector3d rot2rpy(Matrix3d &rotation_matrix);

/**
 * @brief 欧拉角转旋转矩阵 pass
 * @param zyx 按照z-y-x顺序
 * @return 旋转矩阵
 */
Matrix3d euler2rot(array<double, 3> &zyx);

/**
 * @brief RPY角(固定坐标系)转旋转矩阵，按照绕x-y-z的顺序
 * @param rpy roll绕x,pitch绕y，yaw绕z
 * @return
 */
Matrix3d rpy2rot(array<double, 3> rpy);

/**
 * @brief 旋转向量转化为旋转矩阵
 * @param rotvec 旋转向量
 */
//Matrix3d rotvec2rotMatrix(array<double, 3> rotvec);

/**
 * @brief 旋转向量转化为旋转矩阵 pass
 * @param rotvec
 * @return
 */
Matrix3d rotvec2rotMatrix(Vector3d rotvec);

/**
 * @brief 旋转矩阵转化为旋转向量 pass
 * @param rotMatrix 旋转矩阵转
 */
Vector3d rotMatrix2rotvec(Matrix3d rotMatrix);

/**
 * @brief 旋转向量转RPY,pass
 * @param rotvec 旋转向量
 * @return 数组，rpy，按照rx,ry,rz的旋转顺序
 */
array<double, 3> rotvec2rpy(double rx, double ry, double rz);

/**
 * @brief RPY转旋转向量 pass
 * @param rpy rpy角
 */
array<double, 3> rpy2rotvec(double rx, double ry, double rz);
/**
 * @brief 旋转向量转四元数 pass
 * @param rot_vec
 * @return
 */
Eigen::Quaterniond rotvec2quat(Vector3d &rot_vec);
/**
 * @brief 四元数转旋转向量 pass
 * @param quat
 * @return
 */
Vector3d quat2rot_vec(Eigen::Quaterniond quat);
/**
 * @brief 位姿矢量转齐次矩阵 pass
 * @param pose
 * @return
 */
Matrix4d rot_vec_pose2mat(vector<double> &pose);

Matrix4d rpy_vec_pose2mat(vector<double> &pose);

/**
 * @brief 齐次矩阵转位姿矢量 pass
 * @param mat
 * @return
 */
vector<double> mat2rot_vec_pose(Matrix4d mat);

/**
 * @brief 计算伴随转换矩阵
 * @param mat
 * @return
 */
Matrix<double,6,6> transform_adjoint_matrix(Matrix4d &mat);

/**
 * @brief 把以rpy表示的位姿矢量转换为以旋转向量表示的位姿矢量
 * @param rpy_pose
 * @return
 */

vector<double> rpy_pose2rot_vec_pose(array<double,6> &rpy_pose);
vector<double> rpy_pose2rot_vec_pose(vector<double> &rpy_pose);
vector<double> rot_vec_pose2rpy_pose(vector<double> &rv_pose);
/**
 * @brief 旋转矩阵转换为四元数
 * @param rot
 * @return
 */
Quaterniond matrix2quat(const Matrix3d rot);
Eigen::Matrix3d Quat2RotMat(const Eigen::Quaterniond &q);

#endif